﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Drifting.MotionPlanning.Primitives;


namespace Drifting.Graphics.EffectObjects
{
    public class Skin_G
    {
        public enum PreviewResolution
        {
            Low,
            Median,
            Full
        }

        protected List<State> train;
        protected PreviewResolution preview;
        protected Random rand;
        protected Tetrahedron skin_state;
        protected Tetrahedron skin_state2;
        protected Line skin_edge;
        protected int max_point;
        protected float[] q1_tmp;
        protected float[] q2_tmp;

        public Skin_G(List<State> train, GraphicsDevice device)
        {
            this.train = train;
            preview = PreviewResolution.Low;
            rand = new Random();

            skin_state = new Tetrahedron(0.075f, Color.Yellow, device);
            skin_state2 = new Tetrahedron(0.25f, Color.Yellow, device);
            skin_edge = new Line(0.01f, Color.Blue, device);

            q1_tmp = new float[3];
            q2_tmp = new float[3];

            max_point = 10000;
        }

        public Skin_G(List<State> train, GraphicsDevice device, PreviewResolution preview)
        {
            this.train = train;
            this.preview = preview;
            rand = new Random();

            skin_state = new Tetrahedron(0.075f, Color.Yellow, device);
            skin_edge = new Line(0.01f, Color.Blue, device);

            q1_tmp = new float[3];
            q2_tmp = new float[3];
        }

        public PreviewResolution Preview
        {
            get { return preview; }
            set { preview = value; }
        }

        public void UpdateDataContent(List<State> dataContent)
        {
            lock (train)
            {
                this.train = dataContent;
            }
        }

        public bool isBeingRendered(List<State> train)
        {
            if (train == this.train)
                return true;
            else
                return false;
        }

        virtual public void Draw(Matrix camera, Matrix projection)
        {

            if (train == null) return;
            lock (train)
            {
                int step_size = 1;
                int idx_start = rand.Next(train.Count);

                int step_size_low = (int)(train.Count / max_point);
                if (step_size_low < 1)
                    step_size_low = 1;

                if (preview == PreviewResolution.Low)
                    step_size = step_size_low;
                else if (preview == PreviewResolution.Median)
                {
                    int step_size_probe = (int)Math.Floor(Math.Sqrt(train.Count));
                    if (step_size_probe > step_size_low)
                        step_size = step_size_low;
                    else
                        step_size = step_size_probe;
                }
                else
                    step_size = 1;

                Color colorOfCost = Color.Black;
                double costBound = 7;

                for (int i = idx_start; i < train.Count; i += step_size)
                {
                    State x = train[i];
                    if (TranslateStateForVisualization(x, ref q1_tmp, ref colorOfCost, costBound) == false) continue;
                    skin_state.Draw(camera, projection, q1_tmp, colorOfCost);   //  drawing the states
                }
                for (int i = idx_start - 1; i >= 0; i -= step_size)
                {
                    State x = train[i];
                    if (TranslateStateForVisualization(x, ref q1_tmp, ref colorOfCost, costBound) == false) continue;
                    skin_state.Draw(camera, projection, q1_tmp, colorOfCost);   //  drawing the states
                }
            }

        }

        public void Draw(Matrix camera, Matrix projection, State x_goal)
        {

            if (train == null) return;
            lock (train)
            {
                int step_size = 1;
                int idx_start = rand.Next(train.Count);

                int step_size_low = (int)(train.Count / 5000f);
                if (step_size_low < 1)
                    step_size_low = 1;

                if (preview == PreviewResolution.Low)
                    step_size = step_size_low;
                else if (preview == PreviewResolution.Median)
                {
                    int step_size_probe = (int)Math.Floor(Math.Sqrt(train.Count));
                    if (step_size_probe > step_size_low)
                        step_size = step_size_low;
                    else
                        step_size = step_size_probe;
                }
                else
                    step_size = 1;

                Color colorOfCost = Color.Black;
                double costBound = 7;

                for (int i = idx_start; i < train.Count; i += step_size)
                {
                    State x = train[i];
                    if (TranslateStateForVisualization(x, ref q1_tmp, ref colorOfCost, costBound) == false) continue;
                    skin_state.Draw(camera, projection, q1_tmp, colorOfCost);   //  drawing the states
                }
                for (int i = idx_start - 1; i >= 0; i -= step_size)
                {
                    State x = train[i];
                    if (TranslateStateForVisualization(x, ref q1_tmp, ref colorOfCost, costBound) == false) continue;
                    skin_state.Draw(camera, projection, q1_tmp, colorOfCost);   //  drawing the states
                }

                if (x_goal != null)
                {
                    State x_tmp = x_goal;
                    while (x_tmp != null)
                    {

                        TranslateStateForVisualization(x_tmp, ref q1_tmp, ref colorOfCost, costBound);

                        skin_state2.Draw(camera, projection, q1_tmp, Color.Red);   //  drawing the states


                        Edge e_tmp = x_tmp.ParentEdge;
                        if (e_tmp == null) break;
                        x_tmp = e_tmp.entry;
                    }

                }

            }

        }

        virtual public bool TranslateStateForVisualization(State x, ref float[] q_tmp, ref Color colorOfCost, double costBound)
        {
            if (x == null) return false;
            double[] q = x.Q;
            if (q == null) return false;

            double cost = x.Cost;
            double normalizedCost = 1 - cost / costBound;
            double complementaryNormalizedColor = 1-normalizedCost;

            Color lowColor = Color.Green;
            Color highColor = Color.Red;
            colorOfCost.R = (byte)(normalizedCost * lowColor.R + complementaryNormalizedColor * highColor.R);
            colorOfCost.G = (byte)(normalizedCost * lowColor.G + complementaryNormalizedColor * highColor.G);
            colorOfCost.B = (byte)(normalizedCost * lowColor.B + complementaryNormalizedColor * highColor.B);

            q_tmp[0] = (float)q[0];
            q_tmp[1] = (float)q[1];
            if (q.Length >= 3)
                q_tmp[2] = (float)q[2];
            else
                q_tmp[2] = 0;

            return true;
        }
    }
}
